clc
clear
mfilename

param.Tp=3;
A=eye(2)*2;
B=ones(2,1);
u=[0];
sys_order=size(A,2);
input_order=size(B,2);
d=zeros((param.Tp+1)*sys_order+input_order,1);
D=zeros(param.Tp*sys_order,length(d));

for i=1:param.Tp;
    for j=1:i
        in=(i-1)*sys_order+1;
        jn=(j-1)*sys_order+1;
        D(in:in+sys_order-1,jn:jn+sys_order-1)=-A^(i-j+1);
    end
    D(in:in+sys_order-1,(j)*sys_order+1:(j+1)*sys_order)=eye(sys_order);
    pomoc=zeros(size(B));
    for k=1:i
       pomoc=pomoc-A^(k-1)*B; 
    end    
    D(in:in+sys_order-1,end-input_order+1:end)=pomoc;
end
D